#include"gongji/gongji.h"

Gongji::Gongji():
	m_reading_status(false),
	m_pkg_buffer(new uint8_t[500])
{
}

Gongji::~Gongji()
{
	this->closeSerial();
	if(m_pkg_buffer!=NULL)
		delete [] m_pkg_buffer;
}

bool Gongji::openSerial(const std::string& port,int baudrate)
{
	m_serial_port = new serial::Serial(port,baudrate,serial::Timeout::simpleTimeout(10)); 

	if (!m_serial_port->isOpen())
	{
		std::stringstream output;
        output << "Serial port: " << port << " failed to open." << std::endl;
		delete m_serial_port;
		m_serial_port = NULL;
		return false;
	} 
	else 
	{
		std::stringstream output;
		output << "Serial port: " << port << " opened successfully." << std::endl;
	}

	m_serial_port->flush();
	return true;
}

void Gongji::closeSerial()
{
	if(m_serial_port!=NULL)
	{
		delete m_serial_port;
		m_serial_port = NULL;
	}
}

bool Gongji::init()
{
	ros::NodeHandle nh;
	ros::NodeHandle nh_private("~");
	m_pub_rs422 = nh_private.advertise<gps_msgs::Gongji>(nh_private.param<std::string>("gps_topic","/gps"),1);
	// m_pub_wheel = nh.advertise<gps_msgs::Rotation>(nh_private.param<std::string>("rotation_topic","/rotation"),1);
	
	m_pub_ll2utm = nh_private.advertise<nav_msgs::Odometry>(nh_private.param<std::string>("odom_topic","/odom"),1);
	std::cout << nh_private.param<std::string>("odom_topic","/odom") << std::endl;
	nh_private.param<std::string>("parent_frame_id", m_parent_frame_id, "world");
	nh_private.param<std::string>("child_frame_id", m_child_frame_id, "gps");
	
	nh_private.param<bool>("pub_odom", m_is_pub_ll2utm, false);
	nh_private.param<bool>("pub_tf", m_is_pub_tf, false);
	
	std::string port_name = nh_private.param<std::string>("port_name","/dev/ttyUSB0");
	int baudrate = nh_private.param<int>("baudrate",460800);
	if(!openSerial(port_name,baudrate))
		return false;
	return true;
}

void Gongji::startReading()
{
	if(m_reading_status)
		return ;
	m_read_thread_ptr = boost::shared_ptr<boost::thread>(new boost::thread(&Gongji::readSerialThread,this));
}

void Gongji::readSerialThread()
{
	std::cout << "start reading." <<std::endl;
	m_reading_status = true;
	const int Max_read_size = 200;
	uint8_t * const raw_data_buf = new uint8_t[Max_read_size];
	size_t get_len;
		   
	while(ros::ok() && m_reading_status)
	{
		try
		{
			get_len = m_serial_port->read(raw_data_buf,Max_read_size);
		}
		catch(std::exception &e)
		{
			ROS_ERROR("Error reading from serial port: %s",e.what());
			ros::Duration(0.01).sleep();
			continue;
		}
		
		if(get_len == 0) 
		{
			ros::Duration(0.01).sleep();
			continue;
		}
		
		parseIncomingData(raw_data_buf, get_len);
		ros::Duration(0.01).sleep();
		
//		boost::this_thread::sleep(boost::posix_time::milliseconds(10));
	}
	
	delete [] raw_data_buf;
}

void Gongji::parseIncomingData(uint8_t* buffer,size_t len)
{
	// std::cout << " parseIncomingData " <<std::endl;
	static uint8_t pkg_buffer[65];   
	static size_t pkg_buffer_index = 0; //数据包缓存定位索引
	
	size_t pkg_len1 = 0;
	size_t pkg_len2 = 0;
	
	bool crc_effective;
	
	for(size_t i=0; i<len; ++i)
	{
//        std::cout << "check " << std::endl;
		if(0 == pkg_buffer_index) 
		{
			if(0xaa == buffer[i]){
				pkg_buffer[pkg_buffer_index++] = buffer[i];
				
			}
			// std::cout << "pkg_buffer_index: " << pkg_buffer_index << std::endl;
		}
		
		else if(1 == pkg_buffer_index)
		{
			if(0x44 == buffer[i])
			{
				pkg_buffer[pkg_buffer_index++] = buffer[i];
				// std::cout << "0x44" << std::endl;
			}
			else
				pkg_buffer_index = 0;
		}
		
		else if(2 == pkg_buffer_index)   
		{
			if(0xaa == buffer[i])
			{
				pkg_buffer[pkg_buffer_index++] = buffer[i];
			}
			else
				pkg_buffer_index = 0;
		}

		else if(3 == pkg_buffer_index)   
		{
			if(0x45 == buffer[i])
			{
				pkg_buffer[pkg_buffer_index++] = buffer[i];
			}
			else
				pkg_buffer_index = 0;
		}
		
		else
		{
//		    std::cout << "check " << std::endl;
			pkg_buffer[pkg_buffer_index++] = buffer[i];  
			// std::cout << "pkg_buffer_index: " << pkg_buffer_index << std::endl;
			if(pkg_buffer_index == 65)  // 待测试
			{
				// crc-8 
				crc_effective = (cal_crc_table(buffer, 64) == pkg_buffer[64]) ? 1 : 1;
                std::cout << "crc_effective: " << crc_effective << std::endl;
				if(crc_effective)
					parse(pkg_buffer);
				else
					ROS_ERROR("check failed.");
				pkg_buffer_index = 0;
			}
			
		}
	}
}

void Gongji::parse(const uint8_t* buffer)
{
	// std::cout << "parse" << std::endl;
	auto gpsPtr = (const pkgRS422_t *)buffer;


	m_gps.header.stamp = ros::Time::now();
	m_gps.header.frame_id = "gps";
	m_gps.lat = gpsPtr->lat*K1;
	m_gps.lon = gpsPtr->lon*K1;
	m_gps.alt = gpsPtr->alt*K2;
	m_gps.Ve = gpsPtr->Ve*K3;
	m_gps.Vn = gpsPtr->Vn*K3;
	m_gps.Vu = gpsPtr->Vu*K3;
	m_gps.Pitch = gpsPtr->Pitch*K4;
	m_gps.Roll = gpsPtr->Roll*K4;
	m_gps.Yaw = gpsPtr->Yaw*K4;
	m_gps.INS_Status = gpsPtr->INS_Status;
	m_gps.GPSWeek = gpsPtr->GPSWeek;
	m_gps.GPSTime = gpsPtr->GPSTime;
	m_gps.GNSS_Status = gpsPtr->GNSS_Status;
	m_gps.NSV1 = gpsPtr->NSV1;
	m_gps.NSV2 = gpsPtr->NSV2;
	m_gps.online_ax = gpsPtr->online_ax*K5;
	m_gps.online_ay = gpsPtr->online_ay*K5;
	m_gps.online_az = gpsPtr->online_az*K5;
	m_gps.online_gx = gpsPtr->online_gx*K3;
	m_gps.online_gy = gpsPtr->online_gy*K3;
	m_gps.online_gz = gpsPtr->online_gz*K3;
	m_gps.IMU_T = gpsPtr->IMU_T*K6;
	m_gps.Accuracy_HorPositon = gpsPtr->Accuracy_HorPositon*K7;
	m_gps.Accuracy_Height = gpsPtr->Accuracy_Height*K7;
	m_gps.Accuracy_HorVelocity = gpsPtr->Accuracy_HorVelocity*K7;
	m_gps.Accuracy_UpVelocity = gpsPtr->Accuracy_UpVelocity*K7;
	m_gps.Accuracy_HorPosture = gpsPtr->Accuracy_HorPosture*K7;
	m_gps.Accuracy_Yaw = gpsPtr->Accuracy_Yaw*K7;

	// std::cout << "mgps.yaw = " << m_gps.Yaw << std::endl;
	float latitude = m_gps.lat;
	float longitude = m_gps.lon;
	// if(m_gps.gps_state != 0x0f)
	// 	m_gps.Initializing_State = 0;
	// else 
	// 	m_gps.Initializing_State = 1;
	m_pub_rs422.publish(m_gps);  // 发布原始解析程序
	
	geographic_msgs::GeoPoint point;
	point.latitude = m_gps.lat;
	point.longitude = m_gps.lon;
	point.altitude = m_gps.alt;
	std::cout << "mgps.alt = " << m_gps.alt << std::endl;
	geodesy::UTMPoint utm;
	geodesy::fromMsg(point, utm);
	
	//经测试,车头高车尾低时，inspvax.pitch为正 -> 右方为y
    //      左侧低右侧高时，inspvax.roll为负  -> 前方为x
    //      顺时针旋转时，  inspvax.yaw增大   -> 下方为z
    //      前方朝北时，inspvax.yaw为0
    //因此，gps_base系，前方为x，右侧为y，下方为z

    //由于在此驱动程序中使用UTM(东北天)作为直角定位坐标系，所以需要将姿态角也与此系保持一致
	double roll  = deg2rad(m_gps.Roll);
    double pitch = deg2rad(-m_gps.Pitch);
    double yaw   = (deg2rad(m_gps.Yaw)) + M_PI / 2.0;
    m_satelliteNum = m_gps.NSV1;

	Eigen::AngleAxisd xAngle((roll), Eigen::Vector3d::UnitX());
	Eigen::AngleAxisd yAngle((pitch), Eigen::Vector3d::UnitY());
	Eigen::AngleAxisd zAngle((yaw), Eigen::Vector3d::UnitZ());
	
	Eigen::Quaterniond quat = zAngle * yAngle* xAngle;
	quat.normalized();
	
	if(m_is_pub_ll2utm)    // 判断位：如果这个值为true，就发布这个话题消息
	{
		nav_msgs::Odometry odom;
		
		odom.header.stamp = m_gps.header.stamp;
		odom.header.frame_id = m_parent_frame_id;
		odom.child_frame_id  = m_child_frame_id;
		
		odom.pose.pose.position.x = utm.easting;
		odom.pose.pose.position.y = utm.northing;
		odom.pose.pose.position.z = utm.altitude;
		
		odom.pose.covariance[0] = yaw;
		odom.pose.covariance[1] = point.longitude;
		odom.pose.covariance[2] = point.latitude;
		odom.pose.covariance[3] = m_satelliteNum; //卫星个数
		
		//定位有效性9: 有效，0:无效
		odom.pose.covariance[4] = (m_satelliteNum < 20) ? 0 : 4; 
		
		odom.pose.pose.orientation.x = quat.x();
		odom.pose.pose.orientation.y = quat.y();
		odom.pose.pose.orientation.z = quat.z();
		odom.pose.pose.orientation.w = quat.w();
		
		// std::cout<<"m_pub_ll2utm";
		m_pub_ll2utm.publish(odom);
	}
	
	if(m_is_pub_tf)
	{
		geometry_msgs::TransformStamped transformStamped;
		transformStamped.header.stamp = ros::Time::now();
		transformStamped.header.frame_id = m_parent_frame_id;
		transformStamped.child_frame_id = m_child_frame_id;
		transformStamped.transform.translation.x = utm.easting;
		transformStamped.transform.translation.y = utm.northing;
		transformStamped.transform.translation.z = 0.0;

		transformStamped.transform.rotation.x = quat.x();
		transformStamped.transform.rotation.y = quat.y();
		transformStamped.transform.rotation.z = quat.z();
		transformStamped.transform.rotation.w = quat.w();

		m_tf_br.sendTransform(transformStamped);
	}
	
}

void Gongji::stopReading()
{
	m_reading_status = false;
}

int main(int argc,char** argv)
{
	ros::init(argc,argv,"gongji_node");
	
	Gongji gps;
	
	if(gps.init())
	{
		gps.startReading();
		ROS_INFO("%s initial ok...",ros::this_node::getName().c_str());
	}
	
	ros::spin();
}
